#include <Arduino.h>
#include "I2CSlave.h"
#include "SWMonitor.h"

#include "DataStorage.h"
#include "DataStorageStepper.h"

#include "DispatcherHandler.h"

#include "DigitalOut.h"
#include "log.h"
#include "lwmp.h"

#include "SN74AHC595.h"
#include "TB67S109A.h"
#include "SmartBox.h"
#include "SMC.h"
//"PD3",TIMER2B);
//"PB2",TIMER1B);
//"PD5",TIMER0B);
//"PB1",TIMER1A);
//"PB3",TIMER2A);

DigitalOut interruptPin(PB_5);
ti::SN74AHC595 hc595(PD_6,PD_2,PD_3,PD_4);

SMC smc(PB_1,PB_2,PB_3,hc595);
SMCEngine smcEngine(smc,interruptPin);

ISR(TIMER1_COMPA_vect )//中断函数
{
  //m0++;
  // user code here
  if(!smc.mc(0).motionComplete()){
      if(smc.mc(0).up_edge()){
        smc.mc(0).processMovementUpEdge();
      }else{
        smc.mc(0).processMovementDownEdge();
      }
  }
  //smc.mc(0).toggle();
}
ISR(TIMER1_COMPB_vect )//中断函数
{
 // m1++;
  // user code here
  if(!smc.mc(1).motionComplete()){
      smc.mc(1).up_edge()?smc.mc(1).processMovementUpEdge():smc.mc(1).processMovementDownEdge();
  }
  //smc.mc(1).toggle();
}
ISR(TIMER2_COMPA_vect )//中断函数
{
  //m2++;
  // user code here
  if(!smc.mc(2).motionComplete()){
      smc.mc(2).up_edge()?smc.mc(2).processMovementUpEdge():smc.mc(2).processMovementDownEdge();
  }
  //smc.mc(2).toggle();
}
// ISR(TIMER2_COMPB_vect )//中断函数
// {
//     // user code here
//   //counter_D++;
// }
#ifdef __cplusplus
extern "C"
{
#endif
int	fputc(int __c, FILE *__stream)
{
    Serial.print((char)__c);
    return __c==0?EOF:__c;
}
#ifdef __cplusplus
}
#endif



SW1248 sw1248;
SWMonitor sw_monitor(sw1248);

Mail<> mb;

I2CSlave i2c_slave(mb);

DataStorageStepper motor_data(mb);
DataStorage dataStorage(&motor_data);


DispatcherHandler dispatcherHandler(i2c_slave,dataStorage,smcEngine,smc);

SmartBox smb(i2c_slave,dispatcherHandler,mb);

Scheduler* scheduler_tasks[]={
  &sw1248,              //添加任务1: 按键扫描         (Y)
  &sw_monitor,          //添加任务2: 按键状态检测     (Y)
  &dataStorage,         //添加任务2: 数据检查         (Y)
  &smcEngine,           //添加任务4: 运动控制         (Y)
  &dispatcherHandler    //添加任务5: 系统控制         (Y)
};

//间隔一段时间后, 会自动保存寄存器到 EEPROM 中.
void setup() {
  //auto x = _linkage_setup.alloc();

  // put your setup code here, to run once:

  // start serial for output 
  Serial.begin(9600);
  static FILE f;
  stdout = &f;
  log::debug("pro:begin\r\n");
  // tb67s109a_electrical_control_t stepper_motor_set;
  // hc595.begin(stepper_motor_set.data);
  // stepper_motor_set.devices[0].value = 0x20;
  // stepper_motor_set.devices[1].value = 0x20;
  // stepper_motor_set.devices[2].value = 0x20;
  // hc595.updateRegisters(3);
  // delay(200);
  // stepper_motor_set.devices[0].value = 0x60;
  // stepper_motor_set.devices[1].value = 0x60;
  // stepper_motor_set.devices[2].value = 0x60;
  // hc595.updateRegisters(3);
  // SMCClock::generate();
  // SMCClock::set_driver1_clock_enabled(true);
  // //delay(6000);
  // SMCClock::set_driver2_clock_enabled(true);
  // //delay(6000);
  // SMCClock::set_driver3_clock_enabled(true);

  // delay(6000);
  // SMCClock::set_driver1_clock_enabled(false);
  // delay(6000);
  // SMCClock::set_driver2_clock_enabled(false);
  // delay(6000);
  // SMCClock::set_driver3_clock_enabled(false);

  // for(;;){
  //   //log::debug("m1:%lu\r\n",);
  //   delay(1000);
  // }
  // // start sw1248 
  sw1248.begin();
  log::debug("sw1248 begin:0x%x\r\n",sw1248.state().value);
  sw_monitor.attach([]{
    //检测到地址发生改变，设置超时后更新I2C地址
    i2c_slave.begin(sw1248.state().value);
    log::debug("i2c begin new addr:%x\r\n",sw1248.poll());
  });
  

  // start data storage 
  dataStorage.begin();
  log::debug("ds begin:0x%x\r\n",dataStorage.read_data_storage_eof());

  // start i2c
  i2c_slave.begin(sw1248.state().value);
  log::debug("i2c begin:%x\r\n",sw1248.state().value);
  
  i2c_slave.on_request([]{
    //返回主机请求的数据
   // data_storage_reg_read_t& reg = dispatcherHandler.get_reg_data(i2c_slave.get_reg_read_address());
   dispatcherHandler.get_reg_data(i2c_slave.get_reg_read_address());
  });

  i2c_slave.on_receive([](int howMany){
    //处理主机发送的数据
      i2c_slave.handle_on_receive(howMany,dataStorage.module_header_reg().module_name);
      //data_monitor_timeout.attach(&dataStorage,&DataStorage::check_update,30);
      dataStorage.update();
      dataStorage.cleanup();
  });

  SMCClock::generate();
  
  //smb.begin();
  smc.begin();
 
  smcEngine.begin();

  dispatcherHandler.begin();

  log::debug("run tasks\r\n");
}

void loop() {
  // put your main code here, to run repeatedly:
  for(uint8_t i=0;i<sizeof(scheduler_tasks)/sizeof(Scheduler*);i++){
    scheduler_tasks[i]->schedule();
  }
}

void serialEvent() {
  // 当接收到数据时，这个函数会被自动调用
  // cmdline.cleanup();
  // while (Serial.available()) {
  //   char inChar = (char)Serial.read(); // 读取一个字节的数据
  //   cmdline.append(inChar);
  // }
  // cmdline.update();
}